from mavsdk import System
from mavsdk.offboard import VelocityNedYaw
from utils.logger import logger


class DroneController:
    _instance = None  # 单例实例
    task_execute_id = 0

    def __new__(cls, *args, **kwargs):
        if cls._instance is None:
            cls._instance = super(DroneController, cls).__new__(cls)
            cls._instance._initialized = False  # 标记是否已初始化
        return cls._instance

    def __init__(self):
        if not self._initialized:  # 防止重复初始化
            self.drone = System()
            self._initialized = True

    def set_task_execute_id(self, task_execute_id):
        self.task_execute_id = task_execute_id

    def get_task_execute_id(self):
        return self.task_execute_id

    async def connect(self):
        await self.drone.connect(system_address="udp://:14550")
        logger.info("等待无人机连接...")
        async for state in self.drone.core.connection_state():
            if state.is_connected:
                logger.info("无人机已连接")
                break

    async def arm_and_takeoff(self):
        logger.info("解锁无人机...")
        await self.drone.action.arm()
        logger.info("起飞...")
        await self.drone.action.takeoff()

    async def start_offboard_mode(self):
        logger.info("开启 Offboard 模式...")
        await self.drone.offboard.set_velocity_ned(VelocityNedYaw(0.0, 0.0, 0.0, 0.0))  # 初始化速度
        await self.drone.offboard.start()

    async def stop_offboard_mode(self):
        logger.info("关闭 Offboard 模式..")
        await self.drone.offboard.stop()

    async def land(self):
        logger.info("降落...")
        await self.drone.action.land()

    async def return_to_launch(self):
        logger.info("一键返航...")
        await self.drone.action.return_to_launch()

    async def handle_control(self, data):
        x = data.get("x", 0.0)
        y = data.get("y", 0.0)
        h = data.get("h", 0.0)
        await self.drone.offboard.set_velocity_ned(VelocityNedYaw(x, y, -h, 0.0))
        logger.debug(f"控制指令: x={x}, y={y}, h={h}")
